
#include <string.h>
#include <sys/param.h>
#include "freertos/FreeRTOS.h"
#include "freertos/queue.h"
#include "freertos/task.h"
#include "freertos/event_groups.h"
#include "esp_system.h"
#include "esp_wifi.h"
#include "esp_event.h"
#include "esp_log.h"
#include "nvs_flash.h"
#include "esp_netif.h"
#include "protocol_examples_common.h"

#include "lwip/err.h"
#include "lwip/sockets.h"
#include "lwip/sys.h"
#include <lwip/netdb.h>

#include "tmDriver/tmModbus_tcp.h"
#include "tmDriver/tmI2c.h"
#include "tmDriver/tmUdpMulticastTask.h"
#include "tmDriver/tmDefIO.h"
#include "tmDriver/tmGpio.h"
#include "tmApp/tmApp.h"

static const char *TAG = "i2c";
// volatile S_TMARM_UDP_FAST_AGMSG_SETTING tmarmUdpSetting[4];

void app_main(void)
{
    mb_communication_info_t comm_info = { 0 };

    ESP_ERROR_CHECK(nvs_flash_init());
    ESP_ERROR_CHECK(esp_netif_init());
    ESP_ERROR_CHECK(esp_event_loop_create_default());

    /* This helper function configures Wi-Fi or Ethernet, as selected in menuconfig.
     * Read "Establishing Wi-Fi or Ethernet Connection" section in
     * examples/protocols/README.md for more information about this function.
     */
    ESP_ERROR_CHECK(example_connect());

    // xTaskCreate(&mcast_example_task, "mcast_task", 4096, NULL, 5, NULL);


// #if !CONFIG_EXAMPLE_CONNECT_IPV6
//     comm_info.ip_addr_type = MB_IPV4;
// #else
//     comm_info.ip_addr_type = MB_IPV6;
// #endif
//     comm_info.ip_mode = MB_MODE_TCP;

//     comm_info.ip_port = 502;
    // ESP_ERROR_CHECK(modbus_tcp_slave_init(&comm_info));

    // xTaskCreate(&tmUdpMulticastRecAs5600, "Udp Rec", 4096*2, NULL, 5, NULL);
    // QueueHandle_t queueHandlerForStepCommand = xQueueCreate(20, sizeof(S_STEP_TYPE));
    // tmUdpMulticastRecStepCommand(queueHandlerForStepCommand);

    // xTaskCreate(&tmUdpMulticastRecAs5600, "taskName", 4096 * 2, NULL, 5, NULL);

    volatile S_TMARM_UDP_FAST_AGMSG_SETTING *tmarmUdpSetting = malloc(sizeof(S_TMARM_UDP_FAST_AGMSG_SETTING)*6);
    char taskName[] = "tmarm_fastag1";
    for (size_t i = 0; i < 3; i++)
    {
        tmarmUdpSetting[i].which = i+1;
        tmarmUdpSetting[i].queue = xQueueCreate(20, sizeof(S_AngleFastType));
        xTaskCreate(&TmUdpMulticastRecFastAg, taskName, 4096*2, &tmarmUdpSetting[i], 5-i, NULL);
        taskName[13]++;
    }
    
    tmarmUdpSetting[3].which = 4;
    tmarmUdpSetting[3].queue = xQueueCreate(20, sizeof(S_AngleSlowType)); 
    tmarmUdpSetting[4].which = 5;
    tmarmUdpSetting[4].queue = xQueueCreate(5, sizeof(S_AngleReportType));
    tmarmUdpSetting[5].which = 6;
    tmarmUdpSetting[5].queue = xQueueCreate(5, sizeof(S_COMMAND_TYPE));  
    xTaskCreate(&TmUdpMulticastRecSlowAg, "tmarm_slowag", 4096 * 2, &tmarmUdpSetting[3], 1, NULL);

    // xTaskCreate(&TmAppRunningQueueDemo, "tmarm_rec", 4096 * 2, tmarmUdpSetting, 4, NULL);
    xTaskCreatePinnedToCore(&TmAppRunningQueueDemo, "tmarm_rec", 4096 * 2, tmarmUdpSetting, 2, NULL, 1);
    xTaskCreatePinnedToCore(&TmUdpMulticastReportAg, "tmarm_rpo", 4096 * 2, &tmarmUdpSetting[4], 2, NULL, 0);

    xTaskCreatePinnedToCore(&tmUdpMulticastRecCommand, "tmarm_command", 4096 * 2, &tmarmUdpSetting[5], 2, NULL, 0);

    TmGpioMotorInit();
    TmPwmInit(1000);

    vTaskDelay(3000 / portTICK_PERIOD_MS);
    S_COMMAND_TYPE command;
    command.typelocate = COMMAND_LOCATE_AXIS;
    command.l1 = 100;
    command.l2 = -50;
    command.angler = 0;
    command.anglespeed = 10;
    if (xQueueSend(tmarmUdpSetting[5].queue, (void *)&command, 0) != pdPASS)
        ESP_LOGE(TAG, "Queue Send Error");

    vTaskDelay(5000 / portTICK_PERIOD_MS);
    command.l1 = 80;
    command.l2 = -30;
    if (xQueueSend(tmarmUdpSetting[5].queue, (void *)&command, 0) != pdPASS)
        ESP_LOGE(TAG, "Queue Send Error");

    // TmAppMotorDirSet(0);
    // TmPwmChangeFreqX(500);
    // STEP_START_P;
    // vTaskDelay(2000 / portTICK_PERIOD_MS);
    // TmAppMotorDirReset(0);
    // TmPwmChangeFreqX(500);
    // vTaskDelay(2000 / portTICK_PERIOD_MS);
    // STEP_STOP_P;

    // vTaskDelay(5000 / portTICK_PERIOD_MS);

    // TmAppMotorDirSet(1);
    // TmPwmChangeFreqY(500);
    // STEP_START_V;
    // vTaskDelay(2000 / portTICK_PERIOD_MS);
    // TmAppMotorDirReset(1);
    // TmPwmChangeFreqY(500);
    // vTaskDelay(2000 / portTICK_PERIOD_MS);
    // STEP_STOP_V;

    // vTaskDelay(5000 / portTICK_PERIOD_MS);

    // TmAppMotorDirSet(2);
    // TmPwmChangeFreqZ(500);
    // STEP_START_R;
    // vTaskDelay(2000 / portTICK_PERIOD_MS);
    // TmAppMotorDirReset(2);
    // TmPwmChangeFreqZ(500);
    // vTaskDelay(2000 / portTICK_PERIOD_MS);
    // STEP_STOP_R;

    // TmGpioMotorInit();
    // TmMotorDirSetX;
    // TmPwmInit(1000);
    // TmPwmStartAll();
    // vTaskDelay(1000 / portTICK_PERIOD_MS);
    // TmMotorDirResX;
    // vTaskDelay(1000 / portTICK_PERIOD_MS);
    // TmPwmChangeFreqX(10000);
    // vTaskDelay(1000 / portTICK_PERIOD_MS);
    // TmPwmStopAll();
}
